#ifndef _KALMAN_HPP
#define _KALMAN_HPP

#include "camera_class.h"
#include <Eigen/Dense>
#include <bits/types.h>

class kalman{
private:
    Eigen::Matrix<float, 2, 1> State_m;   //状态矩阵
    Eigen::Matrix<float, 2, 2> F_m;       //状态转置矩阵
    Eigen::Matrix<float, 2, 2> Q_m;       //预测过程方差
    Eigen::Matrix<float, 1, 1> R_m;       //测量过程方差
    Eigen::Matrix<float, 2, 1> K_m;       //卡尔曼增益
    Eigen::Matrix<float, 2, 2> P_m;       //协方差矩阵
    Eigen::Matrix<float, 1, 2> H_m;       //观测矩阵
    double last_t{0};
public:
    kalman(){
        reset();
    }

    void reset(){
        Eigen::Matrix<float, 2, 2> F = Eigen::Matrix<float, 2, 2>::Identity();
        F(1,0) = 1;
        Eigen::Matrix<float, 1, 2> H =  {1, 0};

        Q_m(0,0)=0.8;
        Q_m(0,1)=0;
        Q_m(1,0)=0;
        Q_m(1,1)=0.8;
        R_m(0,0) = 1.0f;
        F_m = F;
        H_m = H;
        P_m = Eigen::Matrix<float,2, 2>::Zero();
        State_m = {0,0};
    }

    void update(Eigen::Matrix<float, 1, 1>  Z_k, double t){
        // 设置转移矩阵中的时间项
        for (int i = 1; i < 2; i++) {
            F_m(i - 1, i) = (double)((t-last_t)/100); // 这里的时间项应该给实际程序耗时 s
        }
        last_t = t;

        Eigen::Matrix<float, 2, 1> x_k = F_m * State_m;     //预测
        P_m = F_m * P_m * F_m.transpose() + Q_m;

        //更新
        K_m = P_m * H_m.transpose() * (H_m * P_m * H_m.transpose() + R_m).inverse();  //卡尔曼增益
        State_m = x_k + K_m * (Z_k - H_m * x_k);    //最优估计
        P_m = (Eigen::Matrix<float, 2, 2>::Identity() - K_m * H_m) * P_m;   //更新后验估计
    }

    Eigen::Matrix<float, 2, 1> get_State(){
        return State_m;
    }
};



#endif